#include "Adaptation.h"
#include "amp.h"

//float32 Adaptation::m_fbArray[2][6]  = {{0.01f, 0.0f, -0.01f, 0.0f, 0.0f, 0.0f},
//										{0.0f, 0.01f, 0.0f, -0.01f, 0.0f, 0.0f}};

//float32 Adaptation::m_fbArray[2][6]  = {{1.0f, 0.0f, -0.55f, 0.0f, 0.0f, 0.0f},
//{0.0f, 1.0f, 0.0f, -0.55f, 0.0f, 0.0f}};

float32 Adaptation::m_fbArray[2][6]  = {{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},
{0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}};

float32 Adaptation::fbArray[12]  = {1.0f, 0.0f, -0.55f, 0.0f, 0.0f, 0.0f,
										0.0f, 1.0f, 0.0f, -0.55f, 0.0f, 0.0f};

Adaptation::Adaptation(int32 flag)
{
	m_adaptationFlag = flag;
	m_adaptationStart = false;
	m_adaptationEnd = false;
	m_trialStart = false;
	m_trialEnd = false;
	m_extensionApproach = false;
	m_extensionTipping = false;
	m_currentPhase = Phase::e_approachPhase;
	m_totalPhase = 3;

	//std::vector<float32> tmp;
	//for(int i = 0; i < 2; ++i)
	//{
	//	tmp.assign(m_fbArray[i], m_fbArray[i] + 6);
	//	m_fbMatrix.push_back(tmp);
	//}
}

bool Adaptation::isShiftingAdaptationStart()
{
	return m_adaptationStart;
}

bool Adaptation::isShiftingAdaptationEnd()
{
	return m_adaptationEnd;
}

b2Vec2 Adaptation::MyAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, std::vector<ActualInfo>& actualInfos, std::vector<Phase>& phases, Finger* finger)
{
	if(m_adaptationFlag & e_shiftingUpAdaptationBit)
		return ShiftingUpAdaptation(timeStep, isTouched, recordInfos, finger);
	else if(m_adaptationFlag & e_shiftingDownAdaptationBit)
		return ShiftingDownAdaptation(timeStep, isTouched, recordInfos, finger);
	else if(m_adaptationFlag & e_shiftingLeftAdaptationBit)
		return ShiftingLeftAdaptation(timeStep, isTouched, recordInfos, finger);
	else if(m_adaptationFlag & e_shiftingRightAdaptationBit)
		return ShiftingRightAdaptation(timeStep, isTouched, recordInfos, finger);
	else if(m_adaptationFlag & e_generalAdaptationBit)
		return GeneralAdaptation(timeStep, isTouched, recordInfos, actualInfos, phases, finger);

	return finger->GetPullPointPosition();
}

b2Vec2 Adaptation::ShiftingUpAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, Finger* finger)
{
	RecordInfo recordInfo = recordInfos.at(timeStep);
	std::string flag = recordInfo.GetFlag();
	static b2Vec2 recordedFingerPosition;
	static float32 diff;

	b2Vec2 fingerPosition = finger->GetFingerPosition();
	b2Vec2 pullPointPosition = finger->GetPullPointPosition();

	if(isTouched && !m_adaptationStart && !m_adaptationEnd && flag.compare("2") && (flag.compare("3")))
	{
		std::vector<RecordInfo>::iterator iter_2 = std::find_if(recordInfos.begin(), recordInfos.end(), RecordInfo_flag_equ("2"));
		int timeStep_2 = iter_2 == recordInfos.end() ? recordInfos.size() : iter_2 -> GetTimeStep();
		std::vector<RecordInfo>::iterator iter_3 = std::find_if(recordInfos.begin(), recordInfos.end(), RecordInfo_flag_equ("3"));
		int timeStep_3 = iter_3 == recordInfos.end() ? recordInfos.size() : iter_3 -> GetTimeStep();

		int timeStep_min = timeStep_2 < timeStep_3 ? timeStep_2 : timeStep_3;

		if(timeStep_min < recordInfos.size())
		{
			recordedFingerPosition = recordInfos.at(timeStep_min).GetFingerPosition();
			timeStep = timeStep_min;
		}
		m_adaptationStart = true;
	}

	if(m_adaptationStart && !m_adaptationEnd)
	{
		if(finger->GetFingerBody()->GetLinearVelocity() == b2Vec2(0.0f, 0.0f))
		{
			diff = fingerPosition.y - recordedFingerPosition.y;
			m_adaptationEnd = true;
		}
		timeStep--;
		return b2Vec2(recordedFingerPosition.x, fingerPosition.y);
	}

	if(m_adaptationStart && m_adaptationEnd)
		return b2Vec2(pullPointPosition.x, pullPointPosition.y + diff);

	return pullPointPosition;
}

b2Vec2 Adaptation::ShiftingDownAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, Finger* finger)
{
	RecordInfo recordInfo = recordInfos.at(timeStep);
	std::string flag = recordInfo.GetFlag();
	static b2Vec2 recordedFingerPosition;
	static float32 diff;

	b2Vec2 fingerPosition = finger->GetFingerPosition();
	b2Vec2 pullPointPosition = finger->GetPullPointPosition();

	if(!isTouched && !m_adaptationStart && !m_adaptationEnd && (!flag.compare("2") || !(flag.compare("3"))))
	{
		recordedFingerPosition = recordInfo.GetFingerPosition();
		m_adaptationStart = true;
	}

	if(!isTouched && m_adaptationStart && !m_adaptationEnd)
	{
		timeStep--;
		return b2Vec2(recordedFingerPosition.x, fingerPosition.y - 0.1f);
	}

	if(isTouched && m_adaptationStart && !m_adaptationEnd)
	{
		if(finger->GetFingerBody()->GetLinearVelocity() == b2Vec2(0.0f, 0.0f))
		{
			diff = fingerPosition.y - recordedFingerPosition.y;
			m_adaptationEnd = true;
		}
		timeStep--;
		return b2Vec2(recordedFingerPosition.x, fingerPosition.y);
	}

	if(m_adaptationStart && m_adaptationEnd)
	{
		return b2Vec2(pullPointPosition.x, pullPointPosition.y + diff);
	}

	return pullPointPosition;
}

b2Vec2 Adaptation::ShiftingRightAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, Finger* finger)
{
	static RecordInfo recordedPeak;
	static RecordInfo recordedFirstTouch;
	static b2Vec2 firstTouchPosition;
	static int firstTouchTimeStep;
	static float32 oldY = 0.0f;
	static float32 newY = 0.0f;
	static float32 maxY = 0.0f;
	static float32 diff;

	b2Vec2 fingerPosition = finger->GetFingerPosition();
	b2Vec2 pullPointPosition = finger->GetPullPointPosition();

	if(isTouched && !m_adaptationStart && !m_adaptationEnd && !m_trialStart && !m_trialEnd)
	{
		std::vector<RecordInfo>::iterator iter_2 = std::find_if(recordInfos.begin(), recordInfos.end(), RecordInfo_flag_equ("2"));
		int timeStep_2 = iter_2 == recordInfos.end() ? recordInfos.size() : iter_2 -> GetTimeStep();
		std::vector<RecordInfo>::iterator iter_3 = std::find_if(recordInfos.begin(), recordInfos.end(), RecordInfo_flag_equ("3"));
		int timeStep_3 = iter_3 == recordInfos.end() ? recordInfos.size() : iter_3 -> GetTimeStep();
		int timeStep_min = timeStep_2 < timeStep_3 ? timeStep_2 : timeStep_3;

		recordedFirstTouch = recordInfos.at(timeStep_min);
		recordedPeak = *max_element(recordInfos.begin() + timeStep_min, recordInfos.end(), [](RecordInfo a, RecordInfo b){return a.GetFingerPosition().y < b.GetFingerPosition().y;});

		firstTouchPosition = fingerPosition;
		firstTouchTimeStep = timeStep;

		m_adaptationStart = true;
	}

	if(m_adaptationStart && !m_adaptationEnd && !m_trialStart && !m_trialEnd)
	{
		newY = fingerPosition.y;
		if(newY < oldY)
		{
			maxY = oldY;
			m_trialStart = true;
			newY = 0.0f;
			oldY = 0.0f;
		}
		else
		{
			oldY = newY;
			b2Vec2 ins = pullPointPosition;
			ins -= fingerPosition;
			ins.Normalize();
			pullPointPosition += ins;
			return pullPointPosition;
		}
	}

	if(m_adaptationStart && !m_adaptationEnd && m_trialStart && !m_trialEnd)
	{		
		if(finger->GetFingerBody()->GetLinearVelocity() == b2Vec2(0.0f, 0.0f))
		{
			m_trialEnd = true;
			float32 boxSize = recordedFirstTouch.GetFingerPosition().y;
			float x1 = sqrt(recordedPeak.GetFingerPosition().y * recordedPeak.GetFingerPosition().y - boxSize * boxSize);
			float x2 = sqrt(maxY * maxY - boxSize * boxSize);
			diff = x2 - x1;
			std::cout << diff << std::endl;
		}
		timeStep--;
		return b2Vec2(firstTouchPosition.x, pullPointPosition.y);
	}

	if(m_adaptationStart && !m_adaptationEnd && m_trialStart && m_trialEnd)
	{
		if(finger->GetFingerBody()->GetLinearVelocity() == b2Vec2(0.0f, 0.0f))
		{
			m_adaptationEnd = true;
			timeStep = firstTouchTimeStep;
		}
		else
		{
			timeStep--;
			return b2Vec2(firstTouchPosition.x + diff, firstTouchPosition.y);
		}
	}

	if(m_adaptationStart && m_adaptationEnd && m_trialStart && m_trialEnd)
	{
		return b2Vec2(pullPointPosition.x + diff, pullPointPosition.y);
	}

	return pullPointPosition;
}

b2Vec2 Adaptation::ShiftingLeftAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, Finger* finger)
{
	return ShiftingRightAdaptation(timeStep, isTouched, recordInfos, finger);
}

b2Vec2 Adaptation::GeneralAdaptation(int& timeStep, bool isTouched, std::vector<RecordInfo>& recordInfos, std::vector<ActualInfo>& actualInfos, std::vector<Phase>& phases, Finger* finger)
{
	b2Vec2 fingerPosition = finger->GetFingerPosition();
	b2Vec2 fingerVelocity = finger->GetFingerVelocity();
	b2Vec2 fingerForce = finger->GetFingerReactionForce();
	b2Vec2 pullPointPosition = finger->GetPullPointPosition();

	b2Vec2 fingerPositionRef = recordInfos.at(timeStep).GetFingerPosition();
	b2Vec2 fingerVelocityRef = recordInfos.at(timeStep).GetFingerVelocity();
	b2Vec2 fingerForceRef = recordInfos.at(timeStep).GetFingerForce();
	b2Vec2 pullPointPositionRef = recordInfos.at(timeStep).GetPullPointPosition();

	bool isExtended = false;

	if(m_currentPhase == Phase::e_approachPhase)
	{
		if(isTouched)
		{
			m_currentPhase = PhaseTransition(Phase::e_approachPhase);
			timeStep = phases.at(m_currentPhase).startTimeStep;
			finger->GetFingerBody()->SetLinearVelocity(recordInfos.at(timeStep).GetFingerVelocity());
		}
		else
		{
			if(timeStep >= phases.at(m_currentPhase).endTimeStep)
			{
				isExtended = true;
				timeStep--;
				static b2Vec2 extensionVector;
				if(!m_extensionApproach)
				{
					extensionVector = recordInfos.at(timeStep).GetPullPointPosition();
					extensionVector -= recordInfos.at(timeStep).GetFingerPosition();
					extensionVector.Normalize();
					extensionVector *= 0.05f;
					m_extensionApproach = true;
				}
				pullPointPosition = fingerPosition;
				pullPointPosition += extensionVector;
			}
		}
	}

	if(m_currentPhase == Phase::e_tipPhase)
	{	
		if(timeStep < phases.at(m_currentPhase).endTimeStep)
		{
			float32 diffMatrix[6];
			
			b2Vec2 pDiff = fingerPosition;
			pDiff -= fingerPositionRef;
			//pDiff.Normalize();

			b2Vec2 vDiff = fingerVelocity;
			vDiff -= fingerVelocityRef;
			//vDiff.Normalize();
			//vDiff *= pDiff.Length();

			b2Vec2 fDiff = fingerForce;
			fDiff -= fingerForceRef;

			diffMatrix[0] = pDiff.x;
			diffMatrix[1] = pDiff.y;
			diffMatrix[2] = vDiff.x;
			diffMatrix[3] = vDiff.y;
			diffMatrix[4] = fDiff.x;
			diffMatrix[5] = fDiff.y;

			b2Vec2 delta = Fb_Mul_Diff(m_fbArray, diffMatrix);
			
			//float32 array[6] = {pDiff.x, pDiff.y, vDiff.x, vDiff.y, fDiff.x, fDiff.y};
			//b2Vec2 delta = MultiplyWithAMP(fbArray, array);

			pullPointPosition = pullPointPositionRef;
			pullPointPosition += delta;
		}
		else
		{
			if(fingerVelocity == b2Vec2(0.0f, 0.0f))
			{
				m_currentPhase = PhaseTransition(Phase::e_tipPhase);
				timeStep = phases.at(m_currentPhase).startTimeStep;
			}
			else
			{
				isExtended = true;
				pullPointPosition = fingerPosition;
				timeStep--;
			}
		}
	}

	if(m_currentPhase == Phase::e_pushbackPhase)
	{
		float32 diffMatrix[6];

		b2Vec2 pDiff = fingerPosition;
		pDiff -= fingerPositionRef;
		//pDiff.Normalize();

		b2Vec2 vDiff = fingerVelocity;
		vDiff -= fingerVelocity;
		//vDiff.Normalize();
		//vDiff *= pDiff.Length();

		b2Vec2 fDiff = fingerForce;
		fDiff -= fingerForceRef;

		diffMatrix[0] = pDiff.x;
		diffMatrix[1] = pDiff.y;
		diffMatrix[2] = vDiff.x;
		diffMatrix[3] = vDiff.y;
		diffMatrix[4] = fDiff.x;
		diffMatrix[5] = fDiff.y;

		b2Vec2 delta = Fb_Mul_Diff(m_fbArray, diffMatrix);

		//float32 array[6] = {pDiff.x, pDiff.y, vDiff.x, vDiff.y, fDiff.x, fDiff.y};
		//b2Vec2 delta = MultiplyWithAMP(fbArray, array);

		pullPointPosition = pullPointPositionRef;
		pullPointPosition += delta;
	}

	ActualInfo actualInfo(fingerPosition, pullPointPosition, fingerVelocity, fingerForce, "0", timeStep, actualInfos.size(), m_currentPhase, isExtended);
	actualInfos.push_back(actualInfo);

	return pullPointPosition;
}

int Adaptation::PhaseTransition(int currentPhase)
{
	return (currentPhase + 1) % m_totalPhase;
}

b2Vec2 Adaptation::Fb_Mul_Diff(float32 (*array1)[6], float32* array2)
{
	float32 result[2] = {0.0f, 0.0f};

	for(int i = 0; i< 2; i++)
	{
		for(int j = 0; j < 6; j++)
		{
			result[i] += array1[i][j] * array2[j];
		}
	}

	return b2Vec2(result[0], result[1]);
}

b2Vec2 Adaptation::MultiplyWithAMP(float32 aMatrix[], float32 bMatrix[])
{
	float32 productMatrix[] = {0.0f, 0.0f};

	concurrency::array_view<float32, 2> a(2, 6, aMatrix);
	concurrency::array_view<float32, 2> b(6, 1, bMatrix);
	concurrency::array_view<float32, 2> product(2, 1, productMatrix);

	concurrency::parallel_for_each(
		product.extent, 
		[=](concurrency::index<2> idx) restrict(amp) {
			int row = idx[0];
			int col = idx[1];
			for (int inner = 0; inner < 6; inner++) {
				product[idx] += a(row, inner) * b(inner, col);
			}
	}
	);

	product.synchronize();

	return b2Vec2(product(0,0), product(1,0));
}